﻿using System;
using System.Net;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Documents;
using System.Windows.Ink;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Animation;
using System.Windows.Shapes;

namespace ManPowerEngine
{
	public class Rigidbody : ComponentBase
	{
		//linear properties
		private Vector mVelocity = new Vector();
		private Vector mForces = new Vector();
		private double mMass;

		//angular properties
		private double mAngularVelocity;
		private double mTorque;
		private double mInertia;

		//dimension of this rigidbody
		private double mWidth;
		private double mLength;

		/// <summary>
		/// Initialize a rigidbody with default values.
		/// </summary>
		/// <param name="width">Width in meters</param>
		/// <param name="length">Length in meters</param>
		/// <param name="mass">Mass in Kilograms</param>
		public Rigidbody(double width, double length, double mass)
		{
			//store physical parameters
			mWidth = width;
			mLength = length;
			mMass = mass;
			// for box, moment of inertia = 1/12 * M * (a^2 + b^2)
			mInertia = (1.0f / 12.0f) * ((width * width) + (length * length)) * mass;
		}

		internal void UpdateForce(double timeStep)
		{
			// customized update first
			mGameObject._PhysicsUpdate(timeStep);
			//integrate physics
			//linear

			Vector acceleration = mForces / mMass;
			mVelocity += acceleration * timeStep;
			mForces = Vector.Zero; //clear forces

			//angular
			double angAcc = mTorque / mInertia;
			mAngularVelocity += angAcc * timeStep;
			mTorque = 0; //clear torque
		}

		internal void UpdatePosition(double timeStep)
		{
			mGameObject.Transform.Position += mVelocity * timeStep * PhysicsSystem.PixelPerMeter;
			if (Math.Abs(mAngularVelocity) < 0.01)
				mAngularVelocity = 0;
			mGameObject.Transform.Rotation += mAngularVelocity * timeStep / Math.PI * 180;
		}

		//velocity of a point on body
		public Vector PointVelocity(Vector worldOffset)
		{
			Vector tangent = new Vector(-worldOffset.Y, worldOffset.X);
			return tangent * mAngularVelocity + mVelocity;
		}

		public void AddForce(Vector worldForce, Vector worldOffset)
		{
			//add linar force
			mForces += worldForce;
			//and it's associated torque
			mTorque += Vector.CrossProduct(worldOffset, worldForce);
		}

		/// <summary>
		/// absolute velocity vector
		/// </summary>
		public Vector Velocity
		{
			get
			{
				return mVelocity;
			}
			set
			{
				mVelocity = value;
			}
		}

		/// <summary>
		/// Mass of this rigidbody
		/// </summary>
		public double Mass
		{
			get
			{
				return mMass;
			}
		}

		/// <summary>
		/// Inertia of moment of this rigidbody
		/// </summary>
		public double Inertia
		{
			get
			{
				return mInertia;
			}
		}

		/// <summary>
		/// absolute angular velocity
		/// </summary>
		public double AngularVelocity
		{
			get
			{
				return mAngularVelocity;
			}
		}
	}
}
